package org.sunspotworld;

import com.sun.spot.io.j2me.radiogram.*;
import java.awt.Dimension;
import java.awt.Toolkit;
import java.awt.AWTException;
import java.awt.MouseInfo;
import java.awt.Point;
import java.awt.Robot;

import java.io.*;
import java.util.HashMap;
import java.util.Map;
import javax.microedition.io.*;

public class SunSpotGloveHostApp {

    private static final int HOST_PORT = 100;
    private static final int REFRESH_FREQUENCY = 50;  //Hz
    private static boolean CONNECT_ONCE = false;
    private static boolean DEBUG = true;
    /*******************************************/
    private static final double X_SENSITIVITY_FACTOR = 100000;
    private static final double Y_SENSITIVITY_FACTOR = 150000;
    private static final double FRICTION_FACTOR = 0.75; //slows down the mouse's pointer when no accel is applied
    private static final double ACCEL_FILTER = 0.03; //filters noise on acceleration data
    private static final double VEL_FILTER = 0.001; //filters noise on velocity calculations
    /*******************************************/
    /************Fingers constants**************/
    private static final double[] FINGER_TRANSLATION = {-846, -842, -869, -870};    //centers the neutral position around zero
    private static final double[] FINGER_FILTER = {23, 5, 4, 4};                      //eliminates noise
    private static final double[] FINGER_NORMALIZATION = {28.5, 8, 1, 25};  //sets the amplitute to [0,1]
    /*******************************************/
    private RadiogramConnection conn;
    private Datagram dg;
    Robot robot;
    private DataWindow a0plot, a1plot, a2plot, a3plot;

    /* A properties holder */
    Map<String, String> properties = new HashMap<String, String>();

    public SunSpotGloveHostApp() {

        try {
            this.robot = new Robot();
        } catch (AWTException ex) {
            ex.printStackTrace();
        }
    }

    public void run() {

        if (CONNECT_ONCE) {
            try {
                conn = (RadiogramConnection) Connector.open("radiogram://:" + HOST_PORT);
                dg = conn.newDatagram(conn.getMaximumLength());
            } catch (IOException ex) {
                ex.printStackTrace();
            }
        }

        this.properties.put("xVelocity", "0");
        this.properties.put("yVelocity", "0");

        Dimension dim = Toolkit.getDefaultToolkit().getScreenSize();
        this.properties.put("screenWidth", String.valueOf(dim.getWidth()));
        this.properties.put("screenHeight", String.valueOf(dim.getHeight()));

        /******************************************************************************/
//        a0plot = new DataWindow("Paluch0", 1000 / REFRESH_FREQUENCY);
//        a1plot = new DataWindow("Paluch1", 1000 / REFRESH_FREQUENCY);
//        a2plot = new DataWindow("Paluch2", 1000 / REFRESH_FREQUENCY);
//        a3plot = new DataWindow("Paluch3", 1000 / REFRESH_FREQUENCY);

        a0plot = new DataWindow("acc x", 1000 / REFRESH_FREQUENCY);
        a1plot = new DataWindow("acc y", 1000 / REFRESH_FREQUENCY);
        a2plot = new DataWindow("v x", 1000 / REFRESH_FREQUENCY);
        a3plot = new DataWindow("v y", 1000 / REFRESH_FREQUENCY);



        if (DEBUG) {
            java.awt.EventQueue.invokeLater(new Runnable() {

                public void run() {
                        a0plot.setVisible(true);
                        a1plot.setVisible(true);
                        a2plot.setVisible(true);
                        a3plot.setVisible(true);

                }
            });
        }
        /******************************************************************************/
        while (true) {

            long time = System.currentTimeMillis();

            String rawData = readMessage();

            /* Convert CSV datagram into an array of properties */
            String[] data = rawData.split(";");

            parseProperties(data);

            //debug();

            analyzeData();

            try {
                long czas = Math.max(0, 1000 / REFRESH_FREQUENCY - (System.currentTimeMillis() - time));
                Thread.sleep(czas);
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }
        }
    }

    /* Parses the data string, looking for properties, and update their values in the map this.properties */
    private void parseProperties(String[] data) {

        this.properties.put("isPressed", data[0]);
        this.properties.put("accelX", data[1]);
        this.properties.put("accelY", data[2]);
        this.properties.put("accelZ", data[3]);
        this.properties.put("a0", data[4]);
        this.properties.put("a1", data[5]);
        this.properties.put("a2", data[6]);
        this.properties.put("a3", data[7]);
    }


    /* Processes available information to provide some system imput */
    private void analyzeData() {

        //TODO dokonczyc, rozszerzyc

        normalizeFingers();

        Boolean isPressed = Boolean.parseBoolean(this.properties.get("isPressed"));
        double xAcc = Double.parseDouble(this.properties.get("accelX"));
        double yAcc = Double.parseDouble(this.properties.get("accelY"));
        double zAcc = Double.parseDouble(this.properties.get("accelZ"));
        double a0 = Double.parseDouble(this.properties.get("a0"));
        double a1 = Double.parseDouble(this.properties.get("a1"));
        double a2 = Double.parseDouble(this.properties.get("a2"));
        double a3 = Double.parseDouble(this.properties.get("a3"));
        double vx = Double.parseDouble(this.properties.get("xVelocity"));
        double vy = Double.parseDouble(this.properties.get("yVelocity"));


        if (isPressed) {
            /* Motion enabled */

            Point.Double acceleration = convertAccelData(xAcc, yAcc, zAcc);
            Point newPosition = calculatePosition(acceleration);

            if (DEBUG) {
                System.out.println(a0);
                showGraph(acceleration.x, acceleration.y, 10*vx, 10*vy);
            }

            robot.mouseMove(newPosition.x, newPosition.y);
        } else {
            this.properties.put("xVelocity", "0");
            this.properties.put("yVelocity", "0");
        }

    }

    private void showGraph(double a0, double a1, double a2, double a3) {

        long sampleTime = System.currentTimeMillis();

        a0plot.addData(sampleTime, a0);
        a1plot.addData(sampleTime, a1);
        a2plot.addData(sampleTime, a2);
        a3plot.addData(sampleTime, a3);
    }


    /* Analizes accelerator data, to deduct acceleration in the plane parallel to the screen, independently of the device's tilt.
    Returns acceleration in the screen's coordinates plane */
    private Point.Double convertAccelData(double xAcc, double yAcc, double zAcc) {


        zAcc += 1; /* Bo jest obliczany wzgledem pozycji, jak lezal plasko na stole */

        //System.out.println("Przed konwersja: Xacc: " + xAcc + " Yacc: " + yAcc + " Zacc: " + zAcc);


        yAcc = Math.sqrt(yAcc * yAcc + zAcc * zAcc) - 1;
        //yAcc++;

        xAcc = applyAccelFilter(xAcc);
        yAcc = applyAccelFilter(yAcc);
        zAcc = applyAccelFilter(zAcc);

        //System.out.println("Po konwersji: Xacc: " + xAcc + " Yacc: " + yAcc + " Zacc: " + zAcc);

        return new Point.Double(-xAcc, -yAcc);
    }

    /* Calculates a new mouse's position, using previous position and speed and current sensor data */
    private Point calculatePosition(Point.Double acceleration) {

        int xInitial = MouseInfo.getPointerInfo().getLocation().x;
        int yInitial = MouseInfo.getPointerInfo().getLocation().y;
        double xVelocity = Double.parseDouble(this.properties.get("xVelocity"));
        double yVelocity = Double.parseDouble(this.properties.get("yVelocity"));
        double h = 1.0 / REFRESH_FREQUENCY;
        double xFinal;
        double yFinal;
        //System.out.println("h: " + h);
        System.out.println("Przed obliczeniem: " +
                "(x,y) initial: (" + xInitial + "," + yInitial + "), " +
                "V initial: (" + xVelocity + "," + yVelocity + "), " +
                "a initial: (" + acceleration.x + "," + acceleration.y + ") ");

        xFinal = xInitial + xVelocity * X_SENSITIVITY_FACTOR * h + (acceleration.x * X_SENSITIVITY_FACTOR * h * h) / 2;
        yFinal = yInitial + yVelocity * Y_SENSITIVITY_FACTOR * h + (acceleration.y * Y_SENSITIVITY_FACTOR * h * h) / 2;
        xVelocity += acceleration.x * h;
        yVelocity += acceleration.y * h;

        // Apply friction
        xVelocity = applyFriction(xVelocity, acceleration.x);
        yVelocity = applyFriction(yVelocity, acceleration.y);

        xVelocity = applyVelocityFilter(xVelocity);
        yVelocity = applyVelocityFilter(yVelocity);

        this.properties.put("xVelocity", String.valueOf(xVelocity));
        this.properties.put("yVelocity", String.valueOf(yVelocity));

        System.out.println("Po obliczeniu: " +
                "(x,y) final: (" + xFinal + "," + yFinal + "), " +
                "V final: (" + xVelocity + "," + yVelocity + ") \n");

        /* Bound to the screen */
        xFinal = Math.max(xFinal, 0);
        yFinal = Math.max(yFinal, 0);
        xFinal = Math.min(xFinal, Double.parseDouble(this.properties.get("screenWidth")));
        yFinal = Math.min(yFinal, Double.parseDouble(this.properties.get("screenHeight")));

        //System.out.println("Po normalizacji: " +"(x,y) final: (" + xFinal + "," + yFinal + ") ");

        return new Point((int) xFinal, (int) yFinal);
    }

    private void debug() {
        System.out.println("Xacc: " + this.properties.get("accelX") + " Yacc: " + this.properties.get("accelY") + " Zacc: " + this.properties.get("accelZ"));
    }

    private void normalizeFingers() {

        for (int i = 0; i < 4; i++) {
            Double a = Double.parseDouble(this.properties.get("a" + i));
            a += FINGER_TRANSLATION[i];
            a = Math.abs(a);
            a = applyFilter(a, FINGER_FILTER[i]);
            a = Math.sqrt(a);
            a /= FINGER_NORMALIZATION[i];
            this.properties.put("a" + i, a.toString());
        }
    }

    private double applyFilter(double value, double filter) {
        if( Math.abs(value) < filter) {
            value = 0;
        }
        return value;
    }

    private double applyVelocityFilter(double velocity) {
        return applyFilter(velocity, VEL_FILTER);
    }

    private double applyAccelFilter(double accel) {
        return applyFilter(accel, ACCEL_FILTER);
    }

    private double applyFriction(double velocity, double accel) {

        if (accel == 0) {
            velocity*=FRICTION_FACTOR;
        }
        return velocity;
    }

    private String readMessage() {

        String result = null;

        try {
            if (!CONNECT_ONCE) {
                conn = (RadiogramConnection) Connector.open("radiogram://:" + HOST_PORT);
                dg = conn.newDatagram(conn.getMaximumLength());
            }

            try {
                conn.receive(dg);
                result = dg.readUTF();
            } catch (IOException ex) {
                ex.printStackTrace();
            }

            if (!CONNECT_ONCE) {
                conn.close();
            }

        } catch (IOException ex) {
            ex.printStackTrace();
        }

        return result;
    }

    /**
     * Start up the host application.
     *
     * @param args any command line arguments
     */
    public static void main(String[] args) {

        SunSpotGloveHostApp app = new SunSpotGloveHostApp();
        app.run();
    }
}




